Improved Motion Planning Speed and Safety using Regions of Inevitable Collision

Providing safety guarantees for autonomous robots operating in environments with moving obstacles is a difficult problem, particularly for underactuated systems or systems with drift due to momentum. The conventional approach to replanning in dynamic environments typically computes partial plans within the allotted CPU time and validates explored states through robot-obstacle collision checks. However, this approach cannot provide any safety guarantees for the robot beyond the finite planning horizon. This paper explores the approximate computation of regions of inevitable collision for state validation in a replanning framework for dynamic systems. We present experimental results that demonstrate the effectiveness of this technique in providing dramatically improved safety for partial plans in the domain of an underactuated dynamic vehicle. Figure 1. Top Left: Planning among moving obstacles for an underactuated spacecraft; Bottom Left: Cost maps over time showing the Regions of Inevitable Collision (RIC); Right: Detail of RIC combined cost map visualization. 1 Planning for Dynamic Systems The goal of motion planning is to compute a continuous sequence of controls that enables a robotic system to accomplish a given task while obeying various kinematic and dynamic constraints. Constraints may arise from obstacles that must be avoided in the environment, or from the system dynamics due to momentum conservation, velocity and acceleration bounds, or underactuation. Path planning algorithms simplify the problem by considering only the kinematics of the problem and ignoring the system dynamics. This often allows the problem to be expressed as a search in a lower dimensional space than the full state space of the system. But ultimately, any valid solution must obey all of the constraints. A planning algorithm examines and evaluates possible future control actions until either some goal state is reached, all possible actions have been evaluated, or the planner exceeds some time or memory limitation. For a robot operating in unknown or dynamic environments, the planner is typically iterated and interleaved with motion execution and perception processing. This replanning process typically imposes time constraints on the planner. If the planner is unable to reach the goal state in the allotted time, a partial plan is returned. The robot then executes some segment of the partial plan, while the planning process is iterated. The time limit imposed on the planner induces a planning horizon, or a future time beyond which the planner was unable to examine the state of the system. The planning horizon generally corresponds to the length of the partial plan returned during a particular iteration. The avoidance of stationary obstacles in a motion planning algorithm is often accomplished through the use of a geometric interference detection routine (i.e. collision checking routine). Future states that result in a collision of the robot geometry with obstacle geometry are typically discarded during the search. This ensures that any partial plan made up of states in the search tree will never involve a collision. However, for many dynamic systems, states that are not immediately in collision with an obstacle can still present just as much danger in the context of partial plans. For example, a car-like vehicle with momentum moving at high speed towards a wide brick wall obstacle might not have enough braking distance or maneuverability to avoid impact. Thus, even though the car’s current state is free of collision, the car will inevitably collide regardless of the future control actions applied. 2 Regions of Inevitable Collision States that lead to collision regardless of control action make up a region of inevitable collision or RIC (LaValle and Kuffner (1999, 2001)), alternately referred to as inevitable collision states or ICS (Fraichard (2007)). During planning these states should be avoided along with states which intersect obstacles in order to guarantee safety. A limited horizon planner without inevitable collision checks could possibly select a trajectory with an unsafe terminal state that ultimately leads to a future collision outside of the planning horizon. Consider again the example of a car moving at a constant velocity towards a wall. If a future potential impact with the wall is five seconds away and the planning horizon is four seconds, it is likely that executing the computed partial plan endanger the car. If the car requires four seconds of stopping distance, the next time the planner replans it may already be too late. Clearly in this case the problem could be resolved by increasing the planning horizon to a length safely above the vehicle’s stopping distance. However, the computational resources to extend the planning horizon so may not be available. More importantly, this issue is not so easily resolved in cases with moving obstacles, or systems with more complex ”stopping distances”, such as the underactuated system with drift considered in this paper. In some cases, an arbitrarily long planning horizon may be required to avoid entering a region of inevitable collision, which may impractical or impossible given a time-limited planning architecture. A limited-horizon planner that tests for RIC membership can improve overall system safety by discarding any generated state that would lead to an inevitable collision. Theoretically, if we can guarantee that the system will never enter an RIC then by definition there will always be a safe control action to take (Fraichard (2007)). If the planner can always identify safe actions, then we can guarantee that no collision will occur, regardless of the planning horizon length (assuming perfect information and control). This is the primary significant benefit to using RIC checks during planning. The second important benefit is improved planning efficiency. A planner without regions of inevitable collision checks could potentially generate many branches in the search tree that are ultimately useless because all leaf states lead to collisions. Depending on the size of the regions, the size of the time step, and the discretization of the control inputs, the planner may waste significant resources examining useless paths through regions of inevitable collision. By eliminating these states in advance, a planner with RIC checks can save a great deal of time and memory, depending on how efficiently it can identify those regions. In this paper we present a limited horizon motion planner that uses approximate representations of the regions of inevitable collision in order to improve overall planning safety and speed. The experimental domain is a simulated underactuated spaceship vehicle with momentum that must navigate through a space of moving obstacles, similar to the popular game “Asteroids”. An A* search is performed over possible control actions in order to guide the ship towards a target goal location while avoiding collisions with obstacles. In addition to computing binary regions of inevitable collision (RIC) we also introduce two new concepts: the region of potential collision (RPC), and the region of near-collision (RNC). States in the RPC are those for which some set of control actions exist that lead to a collision, and states in the RNC will result in a collision unless the vehicle acts within a certain limited window of time. The RNC and the RPC represent potentially dangerous states that are scored according to risk during planning. Our experiments have shown that utilizing RIC, RNC, and RPC checks during planning resulted in moderate to significant improvements in both speed and overall safety performance.